/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.AnalogModule;
import edu.wpi.first.wpilibj.Dashboard;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DigitalModule;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStationEnhancedIO.EnhancedIOException;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.SmartDashboard;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.SmartDashboardPacketFactory;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.DriverStationEnhancedIO;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.camera.AxisCamera;

import Robo.*;
/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Team3630Iterative extends IterativeRobot {

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    double defaultSteeringGain = 0.65; // the default value for the steering gain
    double speed, turn;
    // autonomus mode variables
    Timer autoTimer;
    int timeInSeconds;
    // the power profiles for the straight and forked robot path. They are
    // different to let the robot drive more slowly as the robot approaches
    // the fork on the forked line case.
    double forkProfile[] = {0.70, 0.70, 0.55, 0.60, 0.60, 0.50, 0.40, 0.40};
    double straightProfile[] = {0.5, 0.5, 0.4, 0.4, 0.35, 0.4, 0.4, 0.4};
    //double straightProfile[] = {0.7, 0.7, 0.6, 0.6, 0.35, 0.35, 0.35, 0.0};
    double powerProfile[];   // the selected power profile
    boolean straightLine;
    double stopTime; // when the robot should look for end
    boolean goLeft; // direction at fork
    boolean atCross;
    int previousValue;
    double x, y, rotation;
    boolean GetDigitalIn;
    float GetAnalogIn;
    int dashboardCountdown;
    double lastDashDisplayTime = 0;
    // autonomous mode arm variables
    double armOpStartTime = 1.0;        // when to commence arm operation

    double elevatorRaisingSpeed = 1.0;
    double elevatorRaisingDuration = 6.0;   // how long to raise
    double elevatorLoweringSpeed = 0.35;
    double elevatorLoweringDuration = 2.0;
    double crossTime = 0;
    double armLoweringSpeed = 0.0;
    double armLoweringDuration = 0.0;       // how long to lower
    double upperRollerSpeed = 1.0;
    double lowerRollerSpeed = 1.0;
    double clawEjectTime = 13.0;        // when to eject

    // private Gyro gyro;

    private RoboRobot robot;
    private RoboTrimmer trimmer = new RoboTrimmer();

    double leftThrottle = 0;
    double rightThrottle = 0;
    
    public Team3630Iterative() throws EnhancedIOException {
        // create the main robot instance
        robot = new RoboRobot();
        // give all tasks access to it (set the robot member variable)
        RoboTask.robot = robot;
        // give the trimmer access to the robot
        trimmer.robot = robot;

        autoTimer = new Timer();
        robot.drive.resetGyro();
    }

    public void robotInit() {
        SmartDashboard.init();
        SmartDashboard.log("Initializing...", "System State");
        robot.start();
    }

    public void SmartDashboardInt() {
        SmartDashboard.log(robot.lineTracker.left, "left sensor input");
        SmartDashboard.log(robot.lineTracker.center, "mid sensor input");
        SmartDashboard.log(robot.lineTracker.right, "right sensor input");
    }

    public void autonomousInit() {
        SmartDashboard.useBlockingIO(true);
        SmartDashboard.log("Auto", "System State");
        robot.start();
        robot.autoPilot.start();
    }

    // NOTE: the documentation is misleading - this is called ONLY when there is some new sensor data
    // (if nothing changes this does NOT get called
    public void autonomousContinuous() {
        robot.handle();
        robot.autoPilot.handle();
    }

    /**
     * This function is called periodically during autonomous (approx. 20msec)
     */
    public void autonomousPeriodic() {
        updateAutoDashboard();
    }


    // Sequence:
    // The entire arm process starts after one second, beginning with raising the elevator at a 0.6 power output level for 2.5 seconds
    // While the elevator is raising, the arm is lowered at the same time at a 0.3 power output level for 2.5 seconds
    // After the arm and elevator have been moved, the rollers push the tube out after 10.0 seconds.
    // NOTE: For the rollers, although the time of activation is 10.0 seconds, due to the programming of the armOpStartTime, the timing is pushed back by a second, I.E. 10.0 seconds = 11.0 seconds
    // display autonomous mode dashboard data

    private void updateAutoDashboard() {
        // update only if it hasn't been updated for at least 0.1 sec

        SmartDashboard.log(autoTimer.get(), "Auto drive T:");
        if (true || ((lastDashDisplayTime + 0.5) < autoTimer.get())) {
            lastDashDisplayTime = autoTimer.get();

            SmartDashboardInt();

            SmartDashboard.log(robot.autoPilot.getTaskTime(), "Autopilot time:");
            SmartDashboard.log(robot.drive.xSpeed, "X speed:");
            SmartDashboard.log(robot.drive.ySpeed, "Y speed:");
            SmartDashboard.log(robot.drive.rotSpeed, "R speed:");
        }
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopInit() {
        SmartDashboard.log("Teleop...", "System State");
        robot.start();
        robot.coPilot.start();
    }

    public void teleopPeriodic() {
        SmartDashboard.log((robot.lineTracker.left?"(X)":"( )") + (robot.lineTracker.center?"-(X)-":"-( )-") + (robot.lineTracker.right?"-(X)":"-( )"), "Line tracker:");
        SmartDashboard.log(robot.manipulator.elevatorEncoder.get(), "Elevator encoder:");
        SmartDashboard.log(robot.manipulator.armEncoder.get(), "Arm encoder:");
    }

    public void teleopContinuous() {
        // Read Joysticks and set drive direction
        robot.handle();
        robot.coPilot.handle();

/*
        //handle elevator buttons
        if (leftDriveStick.getRawButton(3)) {
            SmartDashboard.log("Up", "Elevator");
            elevator.set(elSpeed);
        } else if (leftDriveStick.getRawButton(2)) {
            SmartDashboard.log("Down", "Elevator");
            elevator.set(-elSpeed/2);
        } else {
            SmartDashboard.log("Stop", "Elevator");
            elevator.set(0.0);
        }

        //Handle Manipulator Buttons

        // shift and scale (twist is -1 .. +1, we map it onto 0..1 range
        double armSpeed = 1 - (rightDriveStick.getTwist() + 1) / 2.0;
        SmartDashboard.log(armSpeed, "Arm rotation speed");
        if (rightDriveStick.getRawButton(2)) {
            SmartDashboard.log("Down", "Arm Rotation");
            manipulator.set(-armSpeed/2);
        } else if (rightDriveStick.getRawButton(3)) {
            SmartDashboard.log("Up", "Arm Rotation");
            manipulator.set(armSpeed);
        } else {
            SmartDashboard.log("Stop", "Arm Rotation");
            manipulator.set(0.0);
        }

        //Handle Roller Movements


        if (leftDriveStick.getRawButton(4)) {
            SmartDashboard.log("Out", "Tube");
            rolleralpha.set(1.0);
            rollerbeta.set(1.0);
        } else if (leftDriveStick.getRawButton(5)) {
            SmartDashboard.log("In", "Tube");
            rolleralpha.set(-1.0);
            rollerbeta.set(-1.0);
        } else {
            SmartDashboard.log("Stop", "Tube");
            rolleralpha.set(0.0);
            rollerbeta.set(0.0);
        }

        SmartDashboard.log(robot.drive.readGyro(), "Gyro angle");
 */
 }

    public void disabledInit() {
        SmartDashboard.log("Disabled...", "System State");
        robot.autoPilot.stop();
        robot.coPilot.stop();
    }
    public void disabledContinuous() { // robot is disabled, stop driving
    }

    public void disabledPeriodic() { // robot is disabled, stop driving
        // handle trimmer adjustments

        if(robot.driverStation.leftDriveStick.getRawButton(6)){
            trimmer.next();
            // wait for button release
            while(robot.driverStation.leftDriveStick.getRawButton(6));
        }
        if(robot.driverStation.leftDriveStick.getRawButton(7)){
            trimmer.previous();
            // wait for button release
            while(robot.driverStation.leftDriveStick.getRawButton(7));
        }
        if(robot.driverStation.leftDriveStick.getRawButton(11)){
            trimmer.change(+0.05);
            // wait for button release
            while(robot.driverStation.leftDriveStick.getRawButton(11));
        }
        if(robot.driverStation.leftDriveStick.getRawButton(10)){
            trimmer.change(-0.05);
            // wait for button release
            while(robot.driverStation.leftDriveStick.getRawButton(10));
        }
        SmartDashboard.log(trimmer.name(), "Trimmer variable:");
        SmartDashboard.log(trimmer.value(), "Trimmer value:");

        SmartDashboard.log(robot.manipulator.elevatorEncoder.get(), "Elevator encoder:");
        SmartDashboard.log(robot.manipulator.armEncoder.get(), "Arm encoder:");
    }
}
